#!/usr/bin/env python
# -*- coding:utf-8 -*-

from queue import PriorityQueue

from sklearn.metrics import euclidean_distances
from planner.planner import Planner

class HybridAStarPlanner(Planner):
    """
    参考：https://www.redblobgames.com/pathfinding/a-star/introduction.html
    """
    def __init__(self, graph=None, start=None, goal=None):
        super(HybridAStarPlanner, self).__init__(graph, start, goal)
        self.cost_so_far = {}
        self.came_from = {}
        self.head = {} #x,y,yaw

    def _reset(self):
        self.came_from[self.start] = None
        self.cost_so_far[self.start] = 0

    def current_cost():
        pass

    def field(self):
        base_field()
        edge_of_GDV()
        max_range()

    def heuristic(self):
        a = non_holomotic_without_obstacle()
        b = max(euclidean_distances(), a)
        c = minimum_cost_to_goal()
        heur = max(b, c)

    def a_star_search(self):
        init_start()
        init_goal()
        build_tree()
        
    def smooting_path(self):
        pass

    def plan(self, graph=None, start=None, goal=None):
        self.a_star_search()
        self.smoothing_path()

if __name__ == "__main__":
    pass